////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "MovableEntity.hpp"

#include "../Game/DataManager.hpp"
#include "EntityManager.hpp"

#include <SmegTools/Debug/DebugNew.hpp>

////////////////////////////////////////////////////////////
/// Defines
////////////////////////////////////////////////////////////
#define SMOOTH_UPDATE 

namespace smeg {
    namespace core {
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
MovableEntity::MovableEntity() :
    BaseEntity(),
    m_bTextureFollowDirection( true ),
    m_Velocity( 0.0f, 0.0f ),
    m_Heading( 1.0f, 0.0f ),
    m_Side( 0.0f, 1.0f ),
    m_fMass( 1.0f ),
    m_fMaxSpeed( 1.0f ),
    m_fMaxSpeedSq( 1.0f ),
    m_fMaxForce( 1.0f ),
    m_fMaxForceSq( 1.0f ),
    m_fMaxTurnRate( 360.0f ),
    m_HeadingSmoother( 10, sf::Vector2f( 0.0f, 0.0f ) ),
    m_SmoothedHeading( m_Heading ),
    m_bIsSmoothingOn( false )
{
}
    
////////////////////////////////////////////////////////////
/// Constructor with a texture
////////////////////////////////////////////////////////////
MovableEntity::MovableEntity( const sf::Texture* _pTexture ) :
    BaseEntity( _pTexture ),
    m_bTextureFollowDirection( true ),
    m_Velocity( 0.0f, 0.0f ),
    m_Heading( 1.0f, 0.0f ),
    m_Side( 0.0f, 1.0f ),
    m_fMass( 1.0f ),
    m_fMaxSpeed( 1.0f ),
    m_fMaxSpeedSq( 1.0f ),
    m_fMaxForce( 1.0f ),
    m_fMaxForceSq( 1.0f ),
    m_fMaxTurnRate( 360.0f ),
    m_HeadingSmoother( 10, sf::Vector2f( 0.0f, 0.0f ) ),
    m_SmoothedHeading( m_Heading ),
    m_bIsSmoothingOn( false )
{
}
    
////////////////////////////////////////////////////////////
/// Constructor with a texture and a rectangle
////////////////////////////////////////////////////////////
MovableEntity::MovableEntity( const sf::Texture* _pTexture, const sf::IntRect& _Rectangle ) :
    BaseEntity( _pTexture, _Rectangle ),
    m_bTextureFollowDirection( true ),
    m_Velocity( 0.0f, 0.0f ),
    m_Heading( 1.0f, 0.0f ),
    m_Side( 0.0f, 1.0f ),
    m_fMass( 1.0f ),
    m_fMaxSpeed( 1.0f ),
    m_fMaxSpeedSq( 1.0f ),
    m_fMaxForce( 1.0f ),
    m_fMaxForceSq( 1.0f ),
    m_fMaxTurnRate( 360.0f ),
    m_HeadingSmoother( 10, sf::Vector2f( 0.0f, 0.0f ) ),
    m_SmoothedHeading( m_Heading ),
    m_bIsSmoothingOn( false )
{
}
    
////////////////////////////////////////////////////////////
/// Copy Constructor
////////////////////////////////////////////////////////////
MovableEntity::MovableEntity( const MovableEntity& _Copy ) :
    BaseEntity( _Copy ),
    m_bTextureFollowDirection( _Copy.m_bTextureFollowDirection ),
    m_Velocity( _Copy.m_Velocity ),
    m_Heading( math::NormalizedCopy( _Copy.m_Velocity ) ),
    m_Side( math::PerpCopy( m_Heading ) ),
    m_fMass( _Copy.m_fMass ),
    m_fMaxSpeed( _Copy.m_fMaxSpeed ),
    m_fMaxSpeedSq( _Copy.m_fMaxSpeed * _Copy.m_fMaxSpeed ),
    m_fMaxForce( _Copy.m_fMaxForce ),
    m_fMaxForceSq( _Copy.m_fMaxForce * _Copy.m_fMaxForce ),
    m_fMaxTurnRate( _Copy.m_fMaxTurnRate ),
    m_HeadingSmoother( 10, sf::Vector2f( 0.0f, 0.0f ) ),
    m_SmoothedHeading( m_Heading ),
    m_bIsSmoothingOn( false )
{
}
    
////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
MovableEntity::~MovableEntity()
{
}

////////////////////////////////////////////////////////////
/// Update the entity
////////////////////////////////////////////////////////////
void MovableEntity::Update( float _fDeltaTime ) {
    move( m_Velocity.x * _fDeltaTime, m_Velocity.y * _fDeltaTime );
}

/*********************** Accessors ************************/
void MovableEntity::ConstrainVelocity() {    
    if ( math::IsZero( m_Velocity ) ) {
        math::Zero( m_Velocity );
        m_Heading = sf::Vector2f( 1.0f, 0.0f );
        math::PerpCopy( m_Heading, m_Side );
    } else {
        math::Truncate( m_Velocity, m_fMaxSpeed );
        math::NormalizedCopy( m_Velocity, m_Heading );
        math::PerpCopy( m_Heading, m_Side );
    }

    if ( m_bTextureFollowDirection ) {
        if ( m_bIsSmoothingOn ) {
            m_SmoothedHeading = m_HeadingSmoother.Update( m_Heading );
            setRotation( math::RAD_TO_DEG * atan2( m_SmoothedHeading.y, m_SmoothedHeading.x ) );
        } else {
            setRotation( math::RAD_TO_DEG * atan2( m_Heading.y, m_Heading.x ) );
        }
    }
}

void MovableEntity::SetVelocity( float _fX, float _fY ) {
    m_Velocity.x = _fX;
    m_Velocity.y = _fY;
    
    ConstrainVelocity();
}

void MovableEntity::SetVelocity( const sf::Vector2f& _Velocity ) {
    SetVelocity( _Velocity.x, _Velocity.y );
}

void MovableEntity::AddVelocity( float _fX, float _fY ) {
    m_Velocity.x += _fX;
    m_Velocity.y += _fY;
    
    ConstrainVelocity();
}

void MovableEntity::AddVelocity( const sf::Vector2f& _Velocity ) {
    AddVelocity( _Velocity.x, _Velocity.y );
}

bool MovableEntity::RotateHeadingToFacePosition( const sf::Vector2f& _TargetPos ) {
    sf::Vector2f HeadingTarget( math::NormalizedCopy( _TargetPos - getPosition() ) );
    float fAngle = math::RAD_TO_DEG * acos( math::Dot( m_Heading, HeadingTarget ) );
    
    if ( fAngle < ZERO_EPSILON ) {
        return true;
    }

    if ( fAngle > m_fMaxTurnRate ) {
        fAngle = m_fMaxTurnRate;
    }

    sf::Transform RotateTransform;
    RotateTransform.rotate( fAngle * math::Sign( m_Heading, HeadingTarget ) );
    m_Heading = RotateTransform.transformPoint( m_Heading.x, m_Heading.y );
    m_Velocity = RotateTransform.transformPoint( m_Velocity.x, m_Velocity.y );
    
    math::Normalize( m_Heading );
    math::PerpCopy( m_Heading, m_Side );

    if ( m_bTextureFollowDirection ) {
        setRotation( math::RAD_TO_DEG * atan2( m_Heading.y, m_Heading.x ) );
    }

    return false;
}

    } // namespace core
} // namespace smeg